/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2024 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/* /!\ work in progress */

#pragma GCC diagnostic ignored "-Wmissing-prototypes"
#include "gfc3d_nonsmooth_Newton_AlartCurnier.h"

#include <assert.h>  // for assert
#include <float.h>   // for DBL_EPSILON
#include <math.h>    // for fabs, INFINITY

#include "siconos_debug.h"  // for DEBUG_PRINTF, DEBUG_...
#ifndef __cplusplus
#include <stdbool.h>  // for true
#endif
#include <stdio.h>   // for NULL, printf, fprintf
#include <stdlib.h>  // for calloc, free, malloc
#include <string.h>  // for memcpy

#include "AlartCurnierGenerated.h"               // for fc3d_AlartCurnierFun...
#include "CSparseMatrix.h"              // for CSparseMatrix, CSpar...
#include "Friction_cst.h"                        // for SICONOS_FRICTION_3D_...
#include "GlobalFrictionContactProblem.h"        // for GlobalFrictionContac...
#include "NumericsMatrix.h"                      // for NM_triplet, Numerics...
#include "NumericsSparseMatrix.h"                // for NSM_new, NumericsSpa...
#include "SiconosBlas.h"                         // for cblas_daxpy, cblas_d...
#include "SolverOptions.h"                       // for SolverOptions, SICON...
#include "fc3d_AlartCurnier_functions.h"         // for computeAlartCurnierJ...
#include "fc3d_nonsmooth_Newton_AlartCurnier.h"  // for fc3d_AlartCurnierFun...
#include "gfc3d_Solvers.h"                       // for gfc3d_nonsmooth_Newt...
#include "gfc3d_compute_error.h"                 // for gfc3d_compute_error
#include "numerics_verbose.h"                    // for numerics_printf_verbose
#include "sanitizer.h"                           // for cblas_dcopy_msan

/* compute psi function */
void ACPsi(GlobalFrictionContactProblem *problem, AlartCurnierFun3x3Ptr computeACFun3x3,
           double *globalVelocity, double *reaction, double *velocity, double *rho,
           double *psi) {
  assert(problem->H->size1 == problem->dimension * problem->numberOfContacts);
  unsigned int m = problem->H->size1;
  unsigned int n = problem->M->size0;
  unsigned int problem_size = n + 2 * m;

  cblas_dscal(problem_size, 0., psi, 1);

  /* -problem->M * globalVelocity + problem->H * reaction + problem->q ==> psi  */
  cblas_dscal(problem_size, 0., psi, 1);

  cblas_dcopy(n, problem->q, 1, psi, 1);
  NM_gemv(1., problem->H, reaction, 1, psi);
  NM_gemv(-1., problem->M, globalVelocity, 1, psi);

  /* -velocity + trans(problem->H) * globalVelocity + problem->b ==> psi + n */
  cblas_daxpy(m, -1., velocity, 1, psi + n, 1);
  cblas_daxpy(m, 1, problem->b, 1, psi + n, 1);
  NM_tgemv(1., problem->H, globalVelocity, 1, psi + n);

  /* compute AC function */
  fc3d_AlartCurnierFunction(m, computeACFun3x3, reaction, velocity, problem->mu, rho,
                            psi + n + m, NULL, NULL);
}

/* init memory for jacobian */
CS_INT initACPsiJacobian(CSparseMatrix *M, CSparseMatrix *H, CSparseMatrix *A,
                         CSparseMatrix *B, CSparseMatrix *J, double rescaling) {
  /* only triplet matrix */
  assert(M->nz >= 0);
  assert(H->nz >= 0);
  assert(A->nz >= 0);
  assert(B->nz >= 0);

  /* M square */
  assert(M->m == M->n);

  /* A & B squares */
  assert(A->m == A->n);
  assert(B->m == B->n);

  assert(A->nz == B->nz);

  /* - M */
  for (int e = 0; e < M->nz; ++e) {
    /* DEBUG_PRINTF("e=%d, M->i[e]=%td, M->p[e]=%td, M->x[e]=%g\n", e, M->i[e], M->p[e],
     * M->x[e]); */
    CHECK_RETURN(CSparseMatrix_entry(J, M->i[e], M->p[e], -M->x[e]));
  }

  /* H */
  assert(M->n == H->m);
  for (int e = 0; e < H->nz; ++e) {
    /* DEBUG_PRINTF("e=%d, H->i[e]=%td, H->p[e] + M->n + A->n=%td, H->x[e]=%g\n", */
    /*              e, H->i[e], H->p[e] + M->n + A->n , H->x[e]); */
    CHECK_RETURN(CSparseMatrix_entry(J, H->i[e], H->p[e] + M->n + A->n, rescaling * H->x[e]));
  }

  /* Ht */
  for (int e = 0; e < H->nz; ++e) {
    CHECK_RETURN(CSparseMatrix_entry(J, H->p[e] + M->m, H->i[e], H->x[e]));
  }

  /* -I */
  for (int e = 0; e < A->m; ++e) {
    CHECK_RETURN(CSparseMatrix_entry(J, e + M->m, e + M->n, -1.));
  }

  /* keep A start indice for update */
  CS_INT Astart = J->nz;

  /* A */
  for (int e = 0; e < A->nz; ++e) {
    CHECK_RETURN(CSparseMatrix_entry(J, A->i[e] + M->m + H->n, A->p[e] + M->n, A->x[e]));
  }

  /* B */
  for (int e = 0; e < B->nz; ++e) {
    CHECK_RETURN(CSparseMatrix_entry(J, B->i[e] + M->m + H->n, B->p[e] + M->n + A->n,
                                     rescaling * B->x[e]));
  }

  return Astart;
}

/* update J with new A and B */
void updateACPsiJacobian(CSparseMatrix *M, CSparseMatrix *H, CSparseMatrix *A,
                         CSparseMatrix *B, CSparseMatrix *J, CS_INT Astart) {
  /* only triplet matrix */
  assert(M->nz >= 0);
  assert(H->nz >= 0);
  assert(A->nz >= 0);
  assert(B->nz >= 0);

  /* M square */
  assert(M->m == M->n);

  /* A & B squares */
  assert(A->m == A->n);
  assert(B->m == B->n);

  assert(J->n == M->n + A->m + B->m);
  assert(J->m == M->m + A->n + B->n);

  assert(J->p);
  assert(J->i);
  assert(J->x);

  if (((Astart + A->nz + B->nz) > J->nzmax)) {
    CHECK_RETURN(cs_sprealloc(J, Astart + A->nz + B->nz));
  }

  /* A */
  J->nz = Astart;

  for (int e = 0; e < A->nz; ++e) {
    if (fabs(A->x[e]) > DBL_EPSILON) {
      J->i[J->nz] = A->i[e] + M->m + H->n;
      J->p[J->nz] = A->p[e] + M->n;
      J->x[J->nz] = A->x[e];
      J->nz++;

      assert(J->nz <= J->nzmax);
    }
  }

  /* B */
  for (int e = 0; e < B->nz; ++e) {
    if (fabs(B->x[e]) > DBL_EPSILON) {
      J->i[J->nz] = B->i[e] + M->m + H->n;
      J->p[J->nz] = B->p[e] + M->n + A->n;
      J->x[J->nz] = B->x[e];
      J->nz++;

      assert(J->nz <= J->nzmax);
    }
  }
}

/* 3D lists blocks => sparse matrix */
void init3x3DiagBlocks(int nc, double *P, CSparseMatrix *R) {
  R->m = 3 * nc;
  R->n = 3 * nc;
  R->x = P;
  R->nz = 9 * nc;
  R->nzmax = R->nz;

  for (int ib = 0; ib < nc; ++ib) {
    for (int j = 0; j < 3; ++j) {
      for (int i = 0; i < 3; ++i) {
        R->i[9 * ib + i + 3 * j] = ib * 3 + i;
        R->p[9 * ib + i + 3 * j] = ib * 3 + j;
      }
    }
  }
}

/*
 */
int _globalLineSearchSparseGP(GlobalFrictionContactProblem *problem,
                              AlartCurnierFun3x3Ptr computeACFun3x3, double *solution,
                              double *direction, double *mu, double *rho, double *F,
                              double *psi, CSparseMatrix *J, double *tmp, double alpha[1],
                              unsigned int maxiter_ls) {
  double inf = 1e10;
  double alphamin = 1e-16;
  double alphamax = inf;

  double m1 = 0.01, m2 = 0.99;

  unsigned int n = (unsigned)NM_triplet(problem->M)->m;

  unsigned int m = problem->H->size1;

  unsigned int problem_size = n + 2 * m;

  // Computation of q(t) and q'(t) for t =0

  double q0 = 0.5 * cblas_ddot(problem_size, psi, 1, psi, 1);

  //  tmp <- J * direction
  cblas_dscal(problem_size, 0., tmp, 1);
  cs_gaxpy(J, direction, tmp);

  double dqdt0 = cblas_ddot(problem_size, psi, 1, tmp, 1);
  DEBUG_PRINTF("dqdt0=%e\n", dqdt0);
  DEBUG_PRINTF("q0=%e\n", q0);

  for (unsigned int iter = 0; iter < maxiter_ls; ++iter) {
    // tmp <- alpha*direction+solution
    cblas_dcopy(problem_size, solution, 1, tmp, 1);
    cblas_daxpy(problem_size, alpha[0], direction, 1, tmp, 1);

    ACPsi(problem, computeACFun3x3, tmp,               /* v */
          tmp + problem->M->size0 + problem->H->size1, /* P */
          tmp + problem->M->size0,                     /* U */
          rho, psi);

    double q = 0.5 * cblas_ddot(problem_size, psi, 1, psi, 1);

    assert(q >= 0);

    double slope = (q - q0) / alpha[0];

    int C1 = (slope >= m2 * dqdt0);
    int C2 = (slope <= m1 * dqdt0);

    DEBUG_PRINTF("C1=%i\t C2=%i\n", C1, C2);
    if (C1 && C2) {
      numerics_printf_verbose(1,
                              "---- GFC3D - NSN_AC - global line search success. Number of ls "
                              "iteration = %i  alpha = %.10e, q = %.10e",
                              iter, alpha[0], q);

      return 0;

    } else if (!C1) {
      alphamin = alpha[0];
    } else {
      // not(C2)
      alphamax = alpha[0];
    }

    if (alpha[0] < inf) {
      alpha[0] = 0.5 * (alphamin + alphamax);
    } else {
      alpha[0] = alphamin;
    }
  }
  numerics_printf_verbose(1,
                          "---- GFC3D - NSN_AC - global line search unsuccessful. Max number "
                          "of ls iteration reached  = %i  with alpha = %.10e",
                          maxiter_ls, alpha[0]);

  return -1;
}

void gfc3d_sparseGlobalAlartCurnierInit(SolverOptions *SO) {}

/* Alart & Curnier solver for sparse global problem */
void gfc3d_nonsmooth_Newton_AlartCurnier(GlobalFrictionContactProblem *problem,
                                         double *reaction, double *velocity,
                                         double *globalVelocity, int *info,
                                         SolverOptions *options) {
  assert(problem);
  assert(reaction);
  assert(velocity);
  assert(info);
  assert(options);

  assert(problem->dimension == 3);

  assert(options->iparam);
  assert(options->dparam);

  assert(problem->q);
  assert(problem->mu);
  assert(problem->M);
  assert(problem->H);

  /* M is square */
  assert(problem->M->size0 == problem->M->size1);
  assert(problem->M->size0 == problem->H->size0);

  unsigned int iter = 0;
  unsigned int itermax = options->iparam[SICONOS_IPARAM_MAX_ITER];
  unsigned int erritermax =
      options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION_FREQUENCY];

  if (erritermax == 0) {
    /* output a warning here */
    erritermax = 1;
  }

  assert(itermax > 0);

  double tolerance = options->dparam[SICONOS_DPARAM_TOL];
  assert(tolerance > 0);

  DEBUG_EXPR(NM_display(problem->M););
  DEBUG_PRINTF("M sizes = %i x %i \n", problem->M->size0, problem->M->size1);
  DEBUG_PRINTF("H sizes = %i x %i \n", problem->H->size0, problem->H->size1);

  /* sparse triplet storage */
  NM_triplet(problem->M);
  NM_triplet(problem->H);

  unsigned int n = (unsigned)NM_triplet(problem->M)->m;

  unsigned int m = problem->H->size1;

  unsigned int problem_size = n + 2 * m;

  double norm_q = cblas_dnrm2(n, problem->q, 1);

  double norm_b = cblas_dnrm2(m, problem->b, 1);

  numerics_printf_verbose(1, "---- GFC3D - NSN_AC - Problem information");
  numerics_printf_verbose(1, "---- GFC3D - NSN_AC - 1-norm of M = %g norm of q = %g ",
                          NM_norm_1(problem->M), norm_q);
  numerics_printf_verbose(1, "---- GFC3D - NSN_AC - inf-norm of M = %g ",
                          NM_norm_inf(problem->M));

  numerics_printf_verbose(1, "---- GFC3D - NSN_AC - 1-norm of H = %g norm of b = %g ",
                          NM_norm_1(problem->H), norm_b);
  numerics_printf_verbose(1, "---- GFC3D - NSN_AC - inf-norm of H = %g ",
                          NM_norm_inf(problem->H));

  DEBUG_EXPR(NumericsMatrix *Mdense =
                 NM_create(NM_DENSE, problem->M->size0, problem->M->size1);
             ; NM_to_dense(problem->M, Mdense);
             // NM_display(Mdense);
             printf("conditioning = %g\n",
                    cond(Mdense->matrix0, problem->M->size0, problem->M->size1)););
  /* DEBUG_PRINTF("norm of q = %e\n", norm_q); */
  /* DEBUG_PRINTF("norm of b = %e\n", norm_b); */

  /* DEBUG_PRINTF("norm of M = %e\n", NM_norm(problem->M)); */
  /* DEBUG_PRINTF("norm of H = %e\n", NM_norm(problem->H)); */

  assert((int)m == problem->numberOfContacts * problem->dimension);

  assert((int)n == problem->H->size0); /* size(velocity) ==
                                        * Htrans*globalVelocity */

  AlartCurnierFun3x3Ptr computeACFun3x3 = NULL;

  switch (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION]) {
    case SICONOS_FRICTION_3D_NSN_FORMULATION_ALARTCURNIER_STD: {
      computeACFun3x3 = &computeAlartCurnierSTD;
      break;
    }
    case SICONOS_FRICTION_3D_NSN_FORMULATION_JEANMOREAU_STD: {
      computeACFun3x3 = &computeAlartCurnierJeanMoreau;
      break;
    };
    case SICONOS_FRICTION_3D_NSN_FORMULATION_ALARTCURNIER_GENERATED: {
      computeACFun3x3 = &fc3d_AlartCurnierFunctionGenerated;
      break;
    }
    case SICONOS_FRICTION_3D_NSN_FORMULATION_JEANMOREAU_GENERATED: {
      computeACFun3x3 = &fc3d_AlartCurnierJeanMoreauFunctionGenerated;
      break;
    }
  }

  if (options->iparam[SICONOS_FRICTION_3D_NSN_MEMORY_ALLOCATED] == 0) {
    /* allocate memory */
    assert(options->dWork == NULL);
    assert(options->iWork == NULL);
    options->dWork = (double *)calloc((m +            /* F */
                                       3 * m +        /* A */
                                       3 * m +        /* B */
                                       m +            /* rho */
                                       problem_size + /* psi */
                                       problem_size + /* rhs */
                                       problem_size + /* tmp2 */
                                       problem_size + /* tmp3 */
                                       problem_size /* solution */),
                                      sizeof(double));

    /* XXX big hack here */
    options->iWork = (int *)malloc((3 * m + /* iA */
                                    3 * m + /* iB */
                                    3 * m + /* pA */
                                    3 * m)  /* pB */
                                   * sizeof(CS_INT));

    options->iparam[SICONOS_FRICTION_3D_NSN_MEMORY_ALLOCATED] = 1;
  }

  assert(options->dWork != NULL);
  assert(options->iWork != NULL);

  double *F = options->dWork;
  double *A = F + m;
  double *B = A + 3 * m;
  double *rho = B + 3 * m;

  double *psi = rho + m;
  double *rhs = psi + problem_size;
  double *tmp2 = rhs + problem_size;
  double *tmp3 = tmp2 + problem_size;
  double *solution = tmp3 + problem_size;

  /* XXX big hack --xhub*/
  CS_INT *iA = (CS_INT *)options->iWork;
  CS_INT *iB = iA + 3 * m;
  CS_INT *pA = iB + 3 * m;
  CS_INT *pB = pA + 3 * m;

  CSparseMatrix A_;
  CSparseMatrix B_;
  CSparseMatrix *J;

  A_.p = pA;
  B_.p = pB;
  A_.i = iA;
  B_.i = iB;

  init3x3DiagBlocks(problem->numberOfContacts, A, &A_);
  init3x3DiagBlocks(problem->numberOfContacts, B, &B_);

  J = cs_spalloc(NM_triplet(problem->M)->n + A_.m + B_.m,
                 NM_triplet(problem->M)->n + A_.m + B_.m,
                 NM_triplet(problem->M)->nzmax + 2 * NM_triplet(problem->H)->nzmax + 2 * A_.n +
                     A_.nzmax + B_.nzmax,
                 1, 1);

  /* needed to build the NumericsMatrix for J */
  NumericsSparseMatrix *NSM_J = NSM_new();
  NSM_J->triplet = J;

  DEBUG_PRINTF("NM_triplet(problem->M)->n= %li\t,NM_triplet(problem->M)->nzmax = %li\n",
               NM_triplet(problem->M)->n, NM_triplet(problem->M)->nzmax);
  DEBUG_PRINTF("NM_triplet(problem->M)->n + A_.m + B_.m = %li\n",
               NM_triplet(problem->M)->n + A_.m + B_.m);
  DEBUG_PRINTF(
      "NM_triplet(problem->M)->nzmax + 2*NM_triplet(problem->H)->nzmax + 2*A_.n + A_.nzmax + "
      "B_.nzmax= %li\n",
      NM_triplet(problem->M)->nzmax + 2 * NM_triplet(problem->H)->nzmax + 2 * A_.n + A_.nzmax +
          B_.nzmax);

  assert(A_.n == problem->H->size1);
  assert(A_.nz == problem->numberOfContacts * 9);
  assert(B_.n == problem->H->size1);
  assert(B_.nz == problem->numberOfContacts * 9);

  fc3d_AlartCurnierFunction(m, computeACFun3x3, reaction, velocity, problem->mu, rho, F, A, B);

  double rescaling = 1e+00;

  CS_INT Astart = initACPsiJacobian(NM_triplet(problem->M), NM_triplet(problem->H), &A_, &B_,
                                    J, rescaling);

  assert(Astart > 0);

  assert(A_.m == A_.n);
  assert(B_.m == B_.n);

  assert(A_.m == problem->H->size1);

  // compute rho here
  for (unsigned int i = 0; i < m; ++i) rho[i] = options->dparam[SICONOS_FRICTION_3D_NSN_RHO];

  DEBUG_EXPR(for (unsigned int i = 0; i < m; ++i) printf("rho[%i]=%e\t", i, rho[i]);
             printf("\n"););

  // direction
  for (unsigned int i = 0; i < problem_size; ++i) rhs[i] = 0.;

  /* NM_J wraps NSM_J, which wraps J */
  NumericsMatrix *NM_J = NM_new();
  NM_fill(NM_J, NM_SPARSE, J->n, J->m, NSM_J);

  info[0] = 1;

  /* update local velocity from global velocity */
  /* an assertion ? */
  /* double norm_globalVelocity = cblas_dnrm2(n,globalVelocity,1); */
  /* double norm_reaction = cblas_dnrm2(m,reaction,1); */
  /* DEBUG_PRINTF("norm of globalVelocity = %e\n", norm_globalVelocity); */
  /* DEBUG_PRINTF("norm of reaction = %e\n", norm_reaction); */
  /* if ((fabs(norm_globalVelocity) < DBL_EPSILON) && (fabs(norm_reaction) < DBL_EPSILON) ) */
  /* { */
  /*   cblas_dcopy(n, problem->q, 1, globalVelocity, 1); */
  /*   int info_solver = NM_gesv(problem->M, globalVelocity, true); */
  /* } */

  cblas_dcopy(m, problem->b, 1, velocity, 1);
  NM_tgemv(1., problem->H, globalVelocity, 1, velocity);

  double norm_velocity = cblas_dnrm2(m, velocity, 1);
  if (fabs(norm_velocity) < DBL_EPSILON) {
    for (unsigned int k = 0; k < m; k++) velocity[k] = 0.0;
  }

  DEBUG_PRINTF("norm of velocity = %e\n", cblas_dnrm2(m, velocity, 1));
  double linear_solver_residual = 0.0;

  DEBUG_EXPR(NV_display(globalVelocity, n););
  DEBUG_EXPR(NV_display(velocity, m););
  DEBUG_EXPR(NV_display(reaction, m););

  /* store the current solution */

  double *globalVelocity_k = solution;
  double *velocity_k = &(solution[n]);
  double *reaction_k = &(solution[n + m]);

  memcpy(globalVelocity_k, globalVelocity, n * sizeof(double));
  memcpy(velocity_k, velocity, m * sizeof(double));
  memcpy(reaction_k, reaction, m * sizeof(double));
  numerics_printf_verbose(1, "---- GFC3D - NSN_AC - Start with tolerance = %g", tolerance);
  while (iter++ < itermax) {
    /* /\* store the current solution *\/ */
    /* for(unsigned int i = 0; i < n; ++i) */
    /* { */
    /*   solution[i] = globalVelocity[i]; */
    /* } */
    /* for(unsigned int i = 0; i < m; ++i) */
    /* { */
    /*   solution[i+n] = velocity[i]; */
    /*   solution[i+n+m] = reaction[i]; */
    /* } */

    /* compute psi */
    ACPsi(problem, computeACFun3x3, globalVelocity_k, reaction_k, velocity_k, rho, psi);

    // cblas_dscal(problem_size, rescaling, psi, 1);

    /* compute A & B */
    fc3d_AlartCurnierFunction(m, computeACFun3x3, reaction_k, velocity_k, problem->mu, rho, F,
                              A, B);
    /* update J */
    updateACPsiJacobian(NM_triplet(problem->M), NM_triplet(problem->H), &A_, &B_, J, Astart);

    /* rhs = -psi */
    cblas_dcopy(problem_size, psi, 1, rhs, 1);
    cblas_dscal(problem_size, -1., rhs, 1);

    /* get compress column storage for linear ops */
    CSparseMatrix *Jcsc = cs_compress(J);

    /* Solve: J X = -psi */
    DEBUG_PRINTF("norm of AA = %e\n", NM_norm_1(NM_J));

    /* Solve: AWpB X = -F. We want to preserve NM_J, but as data has
       been updated we unset the previous backup before */
    // int info_solver = NM_gesv(AA, rhs, true);
    int info_solver = NM_LU_solve(NM_preserve(NM_unpreserve(NM_J)), rhs, 1);

    DEBUG_PRINTF("norm of rhs (direction) = %e\n", cblas_dnrm2(problem_size, rhs, 1));
    if (info_solver > 0) {
      fprintf(stderr, "---- GFC3D - NSN_AC - solver failed info = %d\n", info_solver);
      break;
      // Unreachable code => comment.
      /* info[0] = 2; */
      /* CHECK_RETURN(!CSparseMatrix_check_triplet(NM_triplet(NM_J))); */
    }

    /* Check the quality of the solution */
    if (verbose > 0) {
      cblas_dcopy_msan(problem_size, psi, 1, tmp3, 1);
      NM_gemv(1., NM_J, rhs, 1., tmp3);
      linear_solver_residual = cblas_dnrm2(problem_size, tmp3, 1);
      numerics_printf_verbose(1,
                              "---- GFC3D - NSN_AC iteration %d, linear_solver_residual = %g ",
                              iter, linear_solver_residual);
      /* for the component wise scaled residual: cf mumps &
       * http://www.netlib.org/lapack/lug/node81.html */
    }

    /* DEBUG_EXPR(NV_display(rhs,problem_size);); */

    /* line search */

    double alpha = 1.0;
    // int info_ls = 0; Never used.

    switch (options->iparam[SICONOS_FRICTION_3D_NSN_LINESEARCH]) {
      case SICONOS_FRICTION_3D_NSN_LINESEARCH_NO:
        /* without line search */
        // info_ls = 1;
        break;

      case SICONOS_FRICTION_3D_NSN_LINESEARCH_GOLDSTEINPRICE:
        /* Goldstein Price */
        //      info_ls =
        _globalLineSearchSparseGP(
            problem, computeACFun3x3, solution, rhs, problem->mu, rho, F, psi, Jcsc, tmp2,
            &alpha, options->iparam[SICONOS_FRICTION_3D_NSN_LINESEARCH_MAX_ITER]);
        break;
      /* case SICONOS_FRICTION_3D_NSN_LINESEARCH_ARMIJO: */
      /*   /\* FBLSA *\/ */
      /*   info_ls = frictionContactFBLSA(equation, reaction, velocity, problem->mu, rho, F,
       * Ax, Bx, */
      /*                                  problem->M, problem->q, AWpB, tmp1, tmp2, &alpha,
       * options->iparam[SICONOS_FRICTION_3D_NSN_LINESEARCH_MAX_ITER]); */
      /*   break; */
      default: {
        numerics_error("gfc3d_nonsmooth_Newton_AlartCurnier", "Unknown line search option.\n");
      }
    }

    cs_spfree(Jcsc);
    /* if(!info_ls) */
    /* { */
    /*   cblas_daxpy(problem_size, alpha, rhs, 1, solution, 1); */
    /* } */
    /* else */
    /* { */
    /*   cblas_daxpy(problem_size, 1, rhs, 1., solution, 1); */
    /* } */
    cblas_daxpy(problem_size, alpha, rhs, 1, solution, 1);

    DEBUG_PRINTF("norm of the solution  = %e\n", cblas_dnrm2(problem_size, solution, 1));
    DEBUG_EXPR(NV_display(globalVelocity_k, n););
    DEBUG_EXPR(NV_display(velocity_k, m););
    DEBUG_EXPR(NV_display(reaction_k, m););

    options->dparam[SICONOS_DPARAM_RESIDU] = INFINITY;

    if (!(iter % erritermax)) {
      gfc3d_compute_error(problem, reaction_k, velocity_k, globalVelocity_k, tolerance,
                          options, norm_q, norm_b, &(options->dparam[SICONOS_DPARAM_RESIDU]));
    }

    if (options->dparam[SICONOS_DPARAM_RESIDU] >= tolerance) {
      numerics_printf_verbose(1, "---- GFC3D - NSN_AC iteration %d, residual = %g >= %g", iter,
                              options->dparam[SICONOS_DPARAM_RESIDU], tolerance);
    } else {
      info[0] = 0;
      break;
    }
  }

  memcpy(globalVelocity, solution, n * sizeof(double));
  memcpy(velocity, &solution[n], m * sizeof(double));
  memcpy(reaction, &solution[n + m], m * sizeof(double));

  /* for (unsigned int k; k< n; k++){ */
  /*   globalVelocity[k] =  solution[k] /rescaling; */
  /* } */
  /* for (unsigned int k =0  ; k< m; k++){ */
  /*   reaction[k] =  solution[k+n+m] /rescaling; */
  /* } */

  if (verbose > 0) {
    if (!info[0])
      printf("---- GFC3D - NSN_AC - convergence after %d iterations, residual = %g\n", iter,
             options->dparam[SICONOS_DPARAM_RESIDU]);
    else {
      printf("---- GFC3D - NSN_AC - no convergence after %d iterations, residual = %g\n", iter,
             options->dparam[SICONOS_DPARAM_RESIDU]);
    }
  }

  options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;

#ifdef DUMP_PROBLEM
  if (info[0]) {
    static int file_counter = 0;
    char filename[64];
    printf("GLOBALAC: dumping problem\n");
    snprintf(filename, sizeof(filename), "GLOBALAC_failure%d.dat", file_counter++);
    FILE *file = fopen(filename, "w");
    frictionContact_printInFile(problem, file);
    fclose(file);
  }
#endif

  NM_clear(NM_J);
  free(NM_J);
}

void gfc3d_nsn_ac_set_default(SolverOptions *options) {
  options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION_FREQUENCY] = 1;

  options->iparam[SICONOS_FRICTION_3D_NSN_MPI_COM] = -1;

  options->iparam[SICONOS_FRICTION_3D_NSN_MEMORY_ALLOCATED] = 0; /* > 0 memory is allocated */
  options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] =
      SICONOS_FRICTION_3D_NSN_FORMULATION_JEANMOREAU_STD;
  options->iparam[SICONOS_FRICTION_3D_NSN_LINESEARCH] =
      SICONOS_FRICTION_3D_NSN_LINESEARCH_GOLDSTEINPRICE;
  options->iparam[SICONOS_FRICTION_3D_NSN_LINESEARCH_MAX_ITER] = 1000;

  options->dparam[SICONOS_FRICTION_3D_NSN_RHO] = 1.; /* default rho */
}
